#include "skeleton.h"

Skeleton::Skeleton() {
}

Skeleton::Skeleton(const Skeleton& s) {
   rootPosition = s.rootPosition;
   trOrder = s.trOrder;
   bones = s.bones;
   hierarchy = s.hierarchy;
   father = s.father;
   
   rotation = s.rotation;
   translation0 = s.translation0;
   translation1 = s.translation1;
   scale0 = s.scale0;
   scale1 = s.scale1;
}

Skeleton::Skeleton(Point3D rp, Hierarchy h) {
   rootPosition = rp;
   hierarchy = h;
}

Skeleton::~Skeleton() {
}

void Skeleton::insertSon(string bone, string son) {
   hierarchy[bone].push_back(son);
   father[son] = bone;
}

void Skeleton::insertBone(Bone* b) {
   bones[b->getName()] = *b;
   rotation[b->getName()] = Quat();
   translation0[b->getName()] = Vector3D();
   translation1[b->getName()] = Vector3D();
   scale0[b->getName()] = 1.0;
   scale1[b->getName()] = 1.0;
}

void Skeleton::rotateBone(string bone, Vector3D angles, string order,
      bool isRoot, bool isDeg) {

   string n = bone;
   queue<string> Q;
   Q.push(n);
   Sons sons;

   Vector3D axisX(1.0, 0.0, 0.0);
   Vector3D axisY(0.0, 1.0, 0.0);
   Vector3D axisZ(0.0, 0.0, 1.0);
   
   if (!isRoot) {
      axisX = bones[n].getAxisX();
      axisY = bones[n].getAxisY();
      axisZ = bones[n].getAxisZ();
   }

   while (!Q.empty()) {
      n = Q.front();
      Q.pop();
      
      Vector3D rot = bones[n].getDirection();
      
      for (int i = 0; i < (int) order.size(); i++) {
         if (order[i] == 'X' && angles[0] != 0) {
            rotation[n] = Quat::fromAxisAngle(axisX[0],axisX[1],axisX[2],angles[0]*(3.1415/180.0))*rotation[n];
            
            rot = rotate(angles[0], rot, axisX, isDeg);
            bones[n].setAxisX(rotate(angles[0], bones[n].getAxisX(), axisX, isDeg));
            bones[n].setAxisY(rotate(angles[0], bones[n].getAxisY(), axisX, isDeg));
            bones[n].setAxisZ(rotate(angles[0], bones[n].getAxisZ(), axisX, isDeg));
         } else if (order[i] == 'Y' && angles[1] != 0) {
            
            rotation[n] = Quat::fromAxisAngle(axisY[0],axisY[1],axisY[2],angles[1]*(3.1415/180.0))*rotation[n];
            
            rot = rotate(angles[1], rot, axisY, isDeg);
            bones[n].setAxisX(rotate(angles[1], bones[n].getAxisX(), axisY, isDeg));
            bones[n].setAxisY(rotate(angles[1], bones[n].getAxisY(), axisY, isDeg));
            bones[n].setAxisZ(rotate(angles[1], bones[n].getAxisZ(), axisY, isDeg));
         } else if (order[i] == 'Z' && angles[2] != 0) {
            
            rotation[n] = Quat::fromAxisAngle(axisZ[0],axisZ[1],axisZ[2],angles[2]*(3.1415/180.0))*rotation[n];
            
            rot = rotate(angles[2], rot, axisZ, isDeg);
            bones[n].setAxisX(rotate(angles[2], bones[n].getAxisX(), axisZ, isDeg));
            bones[n].setAxisY(rotate(angles[2], bones[n].getAxisY(), axisZ, isDeg));
            bones[n].setAxisZ(rotate(angles[2], bones[n].getAxisZ(), axisZ, isDeg));
         }
      }

      bones[n].setDirection(rot);

      // Refreshing queue
      sons = getSons(n);
      for (int i = 0; i < (int) sons.size(); i++) {
         Q.push(sons[i]);
      }
   }
}

Segment3D Skeleton::getBoneSegment(int i) {
   
   map<string, Bone>::iterator it = bones.begin();
   advance(it, i);
   
   string currentBone = it->second.getName();
   string currentFather = father[currentBone];
   
   return Segment3D(point(currentBone), point(currentFather));
}

Point3D Skeleton::getJointPos(int i) {
   
   if (i > nBones() or i < 0) {
      cout<<"invalid joint id!!!: "<<i<<endl;
      assert(false);
   }
   
   else if (i == nBones()) return getRootPosition();
   else                    return point(boneName(i));
}

Point3D Skeleton::point(string bone) {
   if (bone == "root") {
      return getRootPosition();
   } else {
      return point(father[bone]) + bones[bone].lenXdir();
   }
}

int Skeleton::getBoneId(string name) {
   
   //~ if (name == "root") return nBones();
   map<string, Bone>::iterator it = bones.find(name);
   if (it == bones.end()) return -1;
   return distance(bones.begin(), it);
}

string Skeleton::boneName(int i) {
   
   map<string, Bone>::iterator it = bones.begin();
   advance(it, i);
   
   if (it == bones.end()) return "";
   return it->second.getName();
}

Quat Skeleton::getBoneQuaternion(int i) {
   
   string name = boneName(i);
   if (name.empty()) return Quat();
   return rotation[name];
}

Vector3 Skeleton::getBoneTranslation0(int i) {
   
   string name = boneName(i);
   if (name.empty()) return Vector3();
   return translation0[name];
}

Vector3 Skeleton::getBoneTranslation1(int i) {
   
   string name = boneName(i);
   if (name.empty()) return Vector3();
   return translation1[name];
}

Number Skeleton::getBoneScale0(int i) {
   
   string name = boneName(i);
   if (name.empty()) return 1.0;
   return scale0[name];
}

Number Skeleton::getBoneScale1(int i) {
   
   string name = boneName(i);
   if (name.empty()) return 1.0;
   return scale1[name];
}

vector<vector<int> > Skeleton::getUndirectedGraph() {
   
   vector<vector<int> > graph;
   graph.resize(nBones()+1);
   // Insert the bones edges
   for (int i=0; i<nBones(); ++i) {
      string namebone = boneName(i); // name of the bone
      vector<string> &sons = hierarchy[namebone];
      for (int j=0; j<sons.size(); ++j) {
         int idSon = getBoneId(sons[j]);
         graph[i].push_back(idSon);
      }
   }
   vector<string> &sons = hierarchy["root"];
   for (int j=0; j<sons.size(); ++j) {
      int idSon = getBoneId(sons[j]);
      graph[nBones()].push_back(idSon);
   }
   
   // Inferir las otras arista cuidando de no inserir elemntos duplicados
   for (unsigned int f=0; f<graph.size(); ++f) {
      const vector<int> &row = graph[f];
      for (unsigned int c=0; c<row.size(); ++c) {
         vector<int> &neighbords = graph[row[c]];
         if (find(neighbords.begin(), neighbords.end(), (int)f) == neighbords.end())
            neighbords.push_back(f);
      }
   }
   
   return graph;
}
